
#include "zf_common_headfile.h"
#include "Image.h"//图像处理
#include "PID.h"//PID 参数
#include "Fuzz_pid.h"//模糊PID
#include "DisPlay.h"//屏幕显示

//UDP链接参数
#define SERVER_IP "192.168.5.224"
#define PORT 12345

#define ROW UVC_HEIGHT
#define COL UVC_WIDTH

int CAR_RUN=0;//发车标志位

int16 encoder_left;//左编码器
int16 encoder_right;//右编码器
//操作的图像数组
uint8_t My_Image[70][155]; 
//电机参数初始化
struct pwm_info motor_1_pwm_info;
struct pwm_info motor_2_pwm_info;
//舵机参数初始化
struct pwm_info servo_pwm_info;
//电调参数
struct pwm_info pwm_1_info;
struct pwm_info pwm_2_info;
//计算舵机占空比
#define SERVO_MOTOR_DUTY(x)         ((float)PWM_DUTY_MAX/(1000.0/(float)SERVO_MOTOR_FREQ)*(0.5+(float)(x)/90.0))
// 定义主板上舵机频率  请务必注意范围 50-300
// 如果要修改，需要直接修改设备树。
#define SERVO_MOTOR_FREQ            (servo_pwm_info.freq) 
// 在设备树中，设置的10000。如果要修改，需要与设备树对应。
#define MOTOR1_PWM_DUTY_MAX    (motor_1_pwm_info.duty_max)       
// 在设备树中，设置的10000。如果要修改，需要与设备树对应。 
#define MOTOR2_PWM_DUTY_MAX    (motor_2_pwm_info.duty_max) 
// 在设备树中，默认设置的10000。如果要修改，需要直接修改设备树。
#define PWM_DUTY_MAX                (servo_pwm_info.duty_max) 
// 定义主板上舵机活动范围 角度                                                     
#define SERVO_MOTOR_L_MAX           (71 )                       
#define SERVO_MOTOR_R_MAX           (101) 

int UDP_Flag=0;
//ADC电压检测
uint16 adc_reg = 0;
float adc_scale = 0;
uint16 battery_vol = 0;
//舵机转角中值
int Mid_Angle=(SERVO_MOTOR_L_MAX+SERVO_MOTOR_R_MAX)/2;
//编码器读取的数值
int Last_Left_Encoder=0;
int Last_Right_Encoder=0;
//输出的编码器数值
int Left_Encoder=0;
int Right_Encoder=0;
int IMU_Times=0;
extern int Imu_Get_Stastic;
int Read_Car_Run=0;
//定时器5ms周期，控制车辆行驶
void pit_callback()
{
    encoder_left=0;
    encoder_right=0;
    //读取编码器的值
    encoder_left  = encoder_get_count(ENCODER_1);
    encoder_right = -encoder_get_count(ENCODER_2);

    //继承上一次数据
    Last_Left_Encoder=encoder_left;
    Last_Right_Encoder=encoder_right;

    //一阶互补滤波
    Left_Encoder=0.2*encoder_left+0.8*Last_Left_Encoder;
    Right_Encoder=0.2*encoder_right+0.8*Last_Right_Encoder;

    if(Imu_Get_Stastic==1)
    {
        Zero_Point_Detect();
        Read_Car_Run=1;
        Imu_Get_Stastic=0;
    }
    else 
    {
        imu660ra_get_acc();//获取加速度计的数据
        imu660ra_get_gyro();//获取陀螺仪数据
        Get_Gyroscope_Pitch();//计算航向角
        Get_Gyroscope_Angle();//计算俯仰角
    }
    // imu660ra_get_acc();//获取加速度计的数据
    // imu660ra_get_gyro();//获取陀螺仪数据
    // IMU_Update(imu660ra_gyro_x,imu660ra_gyro_y,imu660ra_gyro_z,imu660ra_acc_x,imu660ra_acc_y,imu660ra_acc_z);
    //启动电机控制
    Contorl();
}
// //定时器10ms周期读取陀螺仪数据
// void pit_Imu()
// {
//     imu660ra_get_acc();//获取加速度计的数据
//     imu660ra_get_gyro();//获取陀螺仪数据
//     Get_Gyroscope_Pitch();//计算航向角
//     Get_Gyroscope_Angle();//计算俯仰角
// }
void sigint_handler(int signum) 
{
    printf("收到Ctrl+C，程序即将退出\n");
    pwm_set_duty(MOTOR1_PWM, 0);   
    pwm_set_duty(MOTOR2_PWM, 0);   
    pwm_set_duty(PWM_1, 0);   
    pwm_set_duty(PWM_2, 0); 
    exit(0);
}

void cleanup()
{
    printf("程序异常退出，执行清理操作\n");
    // 处理程序退出！！！
    // 这里需要关闭电机，关闭电调等。
    pwm_set_duty(MOTOR1_PWM, 0);   
    pwm_set_duty(MOTOR2_PWM, 0);   
    pwm_set_duty(PWM_1, 0);   
    pwm_set_duty(PWM_2, 0);  
     pwm_set_duty(SERVO_MOTOR1_PWM, 0);
}
uint8   temp_str[] = "seekfree this is udp demo.\r\n";
int main(int, char **)
{
    // 注册清理函数
    // 注册SIGINT信号的处理函数
    signal(SIGINT, sigint_handler);//关闭程序
    atexit(cleanup);//异常状态紧急关闭程序
    //陀螺仪初始化
    imu_get_dev_info();
    // 创建一个定时器10ms周期，回调函数为pit_callback
    pit_ms_init(10, pit_callback);
    // 获取PWM设备信息，电机
    pwm_get_dev_info(MOTOR1_PWM, &motor_1_pwm_info);
    pwm_get_dev_info(MOTOR2_PWM, &motor_2_pwm_info);
    // 获取PWM设备信息，电调
    pwm_get_dev_info(PWM_1, &pwm_1_info);
    pwm_get_dev_info(PWM_2, &pwm_2_info);
    //舵机
    pwm_get_dev_info(SERVO_MOTOR1_PWM, &servo_pwm_info);
    // 初始化屏幕
    ips200_init("/dev/fb0");
    /********************************TCP***************************************************/
    // TCP图传链接
    // if(tcp_client_init(SERVER_IP, PORT) == 0)
    // {
    //     UDP_Flag=1;
    //     printf("tcp_client ok\r\n");
    // }
    // else
    // {
    //     printf("tcp_client error\r\n");
    //     return -1;
    // }
    // seekfree_assistant_interface_init(tcp_client_send_data, tcp_client_read_data);
    // seekfree_assistant_camera_information_config(SEEKFREE_ASSISTANT_MT9V03X, My_Image[0], UVC_WIDTH, 70);
    /********************************UDP***************************************************/
    // 初始化UVC摄像头
    if(uvc_camera_init("/dev/video0") < 0)
    {
        return -1;
    }
    //舵机角度初始化
    pwm_set_duty(SERVO_MOTOR1_PWM, SERVO_MOTOR_DUTY(80.4));//80.4
    //电机PWM初始化为0
    pwm_set_duty(MOTOR1_PWM, 0);   
    pwm_set_duty(MOTOR2_PWM, 0); 
    //无刷电机初始化为0 
    pwm_set_duty(PWM_1, 0);   
    pwm_set_duty(PWM_2, 0);
    //蜂鸣器初始化为低 
    gpio_set_level(BEEP, 0x0);
    //PID参数初始化
    PID_Init();
    while (1)
    {
        // 阻塞式等待，图像刷新
        if(wait_image_refresh() < 0)
        {
            // 摄像头未采集到图像，这里需要关闭电机，关闭电调等。
            exit(0);
            pwm_set_duty(MOTOR1_PWM, 0);   
            pwm_set_duty(MOTOR2_PWM, 0);   
            pwm_set_duty(PWM_1, 0);   
            pwm_set_duty(PWM_2, 0); 
        }
        //将图像数据复制到二维数组中
        memcpy(My_Image, rgay_image, sizeof(uint8_t) * 70 * 155);
        //图像处理函数
        Image_Ses();
        //菜单显示
        Menue_Show();
        //将二维数组数据复制到图像中，再进行显示
        memcpy(rgay_image,My_Image , sizeof(uint8_t) * 70 * 155);
        //电压检测
        adc_reg = adc_convert(ADC_REG_PATH);
        adc_scale = adc_get_scale(ADC_SCALE_PATH);
        battery_vol = adc_reg * adc_scale * 11;
        if(battery_vol<11380)
        {
            gpio_set_level(BEEP, 0x1);
        }
        else 
        {
            gpio_set_level(BEEP, 0x0);
        }
        if(HuanDao_Stas!=0 )
        {
            gpio_set_level(BEEP, 0x1);
        }
        else
        {
            gpio_set_level(BEEP, 0x0);
        }
        //电压检测
        if(FuYa_Key==1)
        {
            // 计算无刷电调转速   （1ms - 2ms）/20ms * 10000（10000是PWM的满占空比时候的值）
            // 无刷电调转速 0%   为 500
            // 无刷电调转速 20%  为 600
            // 无刷电调转速 40%  为 700
            // 无刷电调转速 60%  为 800
            // 无刷电调转速 80%  为 900
            // 无刷电调转速 100% 为 1000
            if(Speed_Target==550 || Speed_Target==500|| Speed_Target==570)
            {
                pwm_set_duty(PWM_1, 750);
                pwm_set_duty(PWM_2, 750);
            }
            else if(Speed_Target==600)
            {
                pwm_set_duty(PWM_1, 750);
                pwm_set_duty(PWM_2, 750);
            }
            else if(Speed_Target==650)
            {
                pwm_set_duty(PWM_1, 750);
                pwm_set_duty(PWM_2, 750);
            }
        }
        else if(FuYa_Key==0)
        {
            pwm_set_duty(PWM_1, 0);
            pwm_set_duty(PWM_2, 0);
        }
        //显示图像在屏幕上，，开启图传时关闭
        if(CAR_RUN==0)
        {
            ips200_show_gray_image(0, 40, rgay_image, 155, 70);
        }
        // ips200_show_gray_image(0, 40, rgay_image, 155, 70);
        // 显示图像到屏幕上  TCP发送图像
        // seekfree_assistant_camera_send();
    }
}